//
// Created by lqk on 2022/2/5.
//

#include "PositionVelocityEstimator.h"

void PositionVelocityEstimator::Update() {

}

void PositionVelocityEstimator::CheaterUpdate() {
    for(int i=0;i<3;i++){
        m_estimatedState->m_position[i]=g_cheaterInfo.m_pos[i];
        m_estimatedState->m_velWorld[i]=g_cheaterInfo.m_vel[i];
        m_estimatedState->m_omegaWorld[i]=g_cheaterInfo.m_omega[i];
    }
    m_estimatedState->m_velBody=m_estimatedState->m_rotation.transpose()*m_estimatedState->m_velWorld;
    m_estimatedState->m_omegaBody=m_estimatedState->m_rotation.transpose()*m_estimatedState->m_omegaWorld;
//    std::cout<<m_estimatedState->m_velBody.transpose()<<std::endl;
//    std::cout<<m_estimatedState->m_velWorld.transpose()<<std::endl;
//    std::cout<<m_estimatedState->m_rotation<<std::endl;
}
